import os
import struct
import serial

DEVICE_PATH = "/dev/input/js0"  # Adjust based on your system

SERIAL_DEVICE_PATH = "/dev/ttyS3"  # Serial port for sending data (adjust if needed)
BAUD_RATE = 115200  # Baud rate for serial communication

# Define joystick min and max values
JOY_MIN = -32768
JOY_MAX = 32767

SCALE_MIN = -400
SCALE_MAX = 400

SCALE_RZ_MIN = 400
SCALE_RZ_MAX = -400

scaled_x = 0
scaled_rz = 0

# Open the serial port once for continuous use
try:
    ser = serial.Serial(SERIAL_DEVICE_PATH, BAUD_RATE, timeout=1)
    print(f"Serial port {SERIAL_DEVICE_PATH} opened successfully.")
except serial.SerialException as e:
    print(f"Error opening serial port: {e}")
    exit()

# Send data to the serial port
def send_serial_data(hex_data):
    try:
        ser.write(hex_data)
        print(f"Sent: {hex_data.hex().upper()}")
    except Exception as e:
        print(f"An error occurred while sending data: {e}")

def send_serial_speed():
    message = bytearray.fromhex("7E 08 00 00 00 00 AA")
    # Pack the scaled X value into two bytes (little-endian)
    scaled_x_bytes = struct.pack('<h', scaled_x)  # '<h' means little-endian 2-byte integer

    # Pack the scaled RZ value into two bytes (little-endian)
    scaled_rz_bytes = struct.pack('<h', scaled_rz)  # '<h' means little-endian 2-byte integer

    message[2] = scaled_rz_bytes[0]  # 3rd byte (index 2)
    message[3] = scaled_rz_bytes[1]  # 4th byte (index 3)      

    # Insert the scaled X value into the 5th and 6th bytes
    message[4] = scaled_x_bytes[0]  # 5th byte (index 4)
    message[5] = scaled_x_bytes[1]  # 6th byte (index 5)  

    send_serial_data(message)        

def send_serial_turn():
    message = bytearray.fromhex("7E 09 00 00 00 00 AA")
    # Pack the scaled X value into two bytes (little-endian)
    scaled_x_bytes = struct.pack('<h', scaled_x)  # '<h' means little-endian 2-byte integer

    # Pack the scaled RZ value into two bytes (little-endian)
    #scaled_rz_bytes = struct.pack('<h', scaled_rz)  # '<h' means little-endian 2-byte integer

    # message[2] = scaled_rz_bytes[0]  # 3rd byte (index 2)
    # message[3] = scaled_rz_bytes[1]  # 4th byte (index 3)      

    # Insert the scaled X value into the 5th and 6th bytes
    message[4] = scaled_x_bytes[0]  # 5th byte (index 4)
    message[5] = scaled_x_bytes[1]  # 6th byte (index 5)  

    send_serial_data(message)       

def RunSpeedTurn():
    message = bytearray.fromhex("7E 0A 00 00 00 00 AA")
    # Pack the scaled X value into two bytes (little-endian)
    scaled_x_bytes = struct.pack('<h', scaled_x)  # '<h' means little-endian 2-byte integer

    # Pack the scaled RZ value into two bytes (little-endian)
    scaled_rz_bytes = struct.pack('<h', scaled_rz)  # '<h' means little-endian 2-byte integer

    if( scaled_x == 0 ):
        send_serial_speed()
        return

    if( scaled_rz == 0 ):
        send_serial_turn()
        return

    message[2] = scaled_rz_bytes[0]  # 3rd byte (index 2)
    message[3] = scaled_rz_bytes[1]  # 4th byte (index 3)      

    # Insert the scaled X value into the 5th and 6th bytes
    message[4] = scaled_x_bytes[0]  # 5th byte (index 4)
    message[5] = scaled_x_bytes[1]  # 6th byte (index 5)  

    send_serial_data(message)            


# Open the device file
with open(DEVICE_PATH, "rb") as f:
    while True:
        event = f.read(8)  # Read 8 bytes per event

        if event:
            time, value, event_type, number = struct.unpack("IhBB", event)
            #print(f"Time: {time}, Value: {value}, Type: {event_type}, Number: {number}")
            # Handle axis movement events (X, Y, Z, RZ)
            if event_type == 2:  # Axis movement event
                if number == 0:  # X axis
                    #print(f"Time: {time}, X Axis Value: {value}")
                    scaled_x = int(((value - JOY_MIN) / (JOY_MAX - JOY_MIN)) * (SCALE_MAX - SCALE_MIN) + SCALE_MIN)
                    print(f"Time: {time}, X-axis value: {scaled_x}, Type: {event_type}, Number: {number}")
                    #send_serial_turn()
                    RunSpeedTurn()

                elif number == 1:  # Y axis
                    print(f"Time: {time}, Y Axis Value: {value}")
                elif number == 2:  # Z axis
                    print(f"Time: {time}, Z Axis Value: {value}")
                elif number == 3:  # RZ axis
                    #print(f"Time: {time}, RZ Axis Value: {value}")
                    scaled_rz = int(((value - JOY_MIN) / (JOY_MAX - JOY_MIN)) * (SCALE_RZ_MAX - SCALE_RZ_MIN) + SCALE_RZ_MIN)
                    print(f"Time: {time}, RZ-axis value: {scaled_rz}, Type: {event_type}, Number: {number}")
                    #send_serial_values()
                    RunSpeedTurn()

            # Handle button press/release events (Button A and Button B)
            elif event_type == 1:  # Button press/release event
                if number == 0:  # Button A
                    #action = "pressed" if value == 1 else "released"
                    #print(f"Time: {time}, Button A {action}")
                    if value == 1:
                        send_serial_data(bytes.fromhex("7E 01 00 00 00 00 AA"))
                elif number == 1:  # Button B
                    #action = "pressed" if value == 1 else "released"
                    #print(f"Time: {time}, Button B {action}")
                    if value == 1:
                        send_serial_data(bytes.fromhex("7E 02 00 00 00 00 AA"))

